#include "atomic_0804_ddpc.h"

// 计算两点之间的距离
double haversineDistance(double lat1, double lon1, double lat2, double lon2)
{
    double R = 6371000.0; // 地球半径(m)
    double phi1 = lat1 * M_PI / 180.0;
    double phi2 = lat2 * M_PI / 180.0;
    double delta_phi = (lat2 - lat1) * M_PI / 180.0;
    double delta_lambda = (lon2 - lon1) * M_PI / 180.0;

    double a = sin(delta_phi / 2) * sin(delta_phi / 2) +
               cos(phi1) * cos(phi2) *
                   sin(delta_lambda / 2) * sin(delta_lambda / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));

    return R * c;
}

// 计算两点之间的方位角
double calculateBearing(double lat1, double lon1, double lat2, double lon2)
{
    double phi1 = lat1 * M_PI / 180.0;
    double phi2 = lat2 * M_PI / 180.0;
    double lambda1 = lon1 * M_PI / 180.0;
    double lambda2 = lon2 * M_PI / 180.0;

    double y = sin(lambda2 - lambda1) * cos(phi2);
    double x = cos(phi1) * sin(phi2) -
               sin(phi1) * cos(phi2) * cos(lambda2 - lambda1);
    double theta = atan2(y, x);

    return fmod(theta * 180.0 / M_PI + 360.0, 360.0);
}

// 向量叉积
Vector3D crossProduct(Vector3D a, Vector3D b)
{
    Vector3D result;
    result.x = a.y * b.z - a.z * b.y;
    result.y = a.z * b.x - a.x * b.z;
    result.z = a.x * b.y - a.y * b.x;
    return result;
}

// 向量点积
double dotProduct(Vector3D a, Vector3D b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

// 向量长度
double vectorLength(Vector3D v)
{
    return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}

// 归一化向量
Vector3D normalizeVector(Vector3D v)
{
    double len = vectorLength(v);
    Vector3D result;
    if (len > 0.0)
    {
        result.x = v.x / len;
        result.y = v.y / len;
        result.z = v.z / len;
    }
    else
    {
        result.x = result.y = result.z = 0.0;
    }
    return result;
}

// 弹道偏差计算函数
int calculateTrajectoryDeviation(TrajectoryDeviationInput *input, TrajectoryDeviationOutput *output)
{
    // 1. 计算基本偏差
    output->velocity_deviation = input->target_velocity - input->missile_velocity;
    output->altitude_deviation = input->target_altitude - input->missile_altitude;

    // 计算角度偏差 (考虑圆周角度)
    double angle_diff = fmod(input->target_angle - input->missile_angle + 360.0, 360.0);
    if (angle_diff > 180.0)
        angle_diff -= 360.0;
    output->angle_deviation = angle_diff;

    // 2. 计算空间偏差
    double distance = haversineDistance(input->waypoint1_lat, input->waypoint1_lon,
                                        input->waypoint2_lat, input->waypoint2_lon);
    double bearing = calculateBearing(input->waypoint1_lat, input->waypoint1_lon,
                                      input->waypoint2_lat, input->waypoint2_lon);

    // 将球面坐标转换为笛卡尔坐标 (简化)
    Vector3D missile_pos, target_pos;
    missile_pos.x = distance * cos(input->missile_angle * M_PI / 180.0);
    missile_pos.y = distance * sin(input->missile_angle * M_PI / 180.0);
    missile_pos.z = input->missile_altitude;

    target_pos.x = distance * cos(bearing * M_PI / 180.0);
    target_pos.y = distance * sin(bearing * M_PI / 180.0);
    target_pos.z = input->target_altitude;

    // 计算横向和纵向偏差
    Vector3D missile_to_target = {target_pos.x - missile_pos.x,
                                  target_pos.y - missile_pos.y,
                                  target_pos.z - missile_pos.z};
    Vector3D missile_direction = {cos(input->missile_angle * M_PI / 180.0),
                                  sin(input->missile_angle * M_PI / 180.0),
                                  0};

    // 纵向偏差 (沿导弹方向)
    output->longitudinal_deviation = dotProduct(missile_to_target, missile_direction);

    // 横向偏差 (垂直于导弹方向)
    Vector3D lateral_dir = crossProduct(missile_direction, (Vector3D){0, 0, 1});
    lateral_dir = normalizeVector(lateral_dir);
    output->lateral_deviation = dotProduct(missile_to_target, lateral_dir);

    // 3. 预测脱靶量 (使用比例导引法)
    double closing_velocity = input->missile_velocity + input->target_velocity * cos(output->angle_deviation * M_PI / 180.0);
    double time_to_go = (closing_velocity > 0.1) ? distance / closing_velocity : 1000.0;

    double lambda = atan2(output->lateral_deviation, output->longitudinal_deviation);
    double sigma = input->missile_angle * M_PI / 180.0;
    double theta = bearing * M_PI / 180.0;

    double N = 3.0; // 导航常数
    double predicted_miss = distance * sin(lambda - sigma) *
                            (1 - pow(cos(lambda - sigma), N - 2) /
                                     (N - 2));
    output->predicted_miss_distance = predicted_miss;

    // 4. 计算所需修正量
    output->max_acceleration = 2 * output->lateral_deviation / (time_to_go * time_to_go);
    output->max_turn_rate = output->max_acceleration / input->missile_velocity * 180.0 / M_PI;

    // 限制在物理约束内
    if (fabs(output->max_acceleration) > input->filtered_acceleration_z)
    {
        output->max_acceleration = (output->max_acceleration > 0) ? input->filtered_acceleration_z : -input->filtered_acceleration_z;
        output->max_turn_rate = output->max_acceleration / input->missile_velocity * 180.0 / M_PI;
    }

    // 5. 计算修正量
    output->missile_velocity = output->velocity_deviation * 0.5;
    output->missile_altitude = output->altitude_deviation * 0.3;
    output->missile_angle = output->angle_deviation * 0.7;

    // 6. 计算路径效率
    output->optimal_path_length = distance;
    output->current_path_length = distance + fabs(output->lateral_deviation) * 0.5;
    output->dt = output->optimal_path_length / output->current_path_length;

    // 7. 计算能量消耗估计
    double drag_coefficient = 0.2;
    double air_density = 1.225 * exp(-input->missile_altitude / 8500.0);
    double drag_force = 0.5 * air_density * input->missile_velocity * input->missile_velocity * drag_coefficient;
    output->energy_consumption = drag_force * output->current_path_length;

    // 8. 计算风险因子
    double turn_radius = input->missile_velocity * input->missile_velocity /
                         (input->filtered_acceleration_z * 9.81);
    double risk = 0.0;
    if (turn_radius < input->max_deviation * 2)
    {
        risk = (input->max_deviation * 2 - turn_radius) / (input->max_deviation * 2);
    }
    output->risk_factor = risk;

    // 9. 计算综合偏差评分
    output->time_to_target = sqrt(output->velocity_deviation * output->velocity_deviation * 0.3 +
                                   output->altitude_deviation * output->altitude_deviation * 0.2 +
                                   output->angle_deviation * output->angle_deviation * 0.2 +
                                   output->lateral_deviation * output->lateral_deviation * 0.3);

    // 10. 时间偏差计算
    output->time_deviation = input->estimated_time - time_to_go;

    // 添加一些复杂的计算和模拟
    for (int i = 0; i < 100; i++)
    {
        // 模拟一些复杂的空气动力学效应
        double mach_number = input->missile_velocity / 340.0;
        double mach_factor = (mach_number > 1.0) ? 1.0 + 0.2 * (mach_number - 1.0) : 1.0;

        // 更新修正量考虑马赫数效应
        output->missile_velocity *= mach_factor;
        output->missile_angle *= mach_factor;

        // 模拟科里奥利力效应
        double coriolis_effect = 2 * 7.2921159e-5 * input->missile_velocity *
                                 sin(input->waypoint1_lat * M_PI / 180.0);
        output->missile_angle += coriolis_effect * 180.0 / M_PI * 0.01;

        // 模拟地球曲率效应
        double earth_curvature = distance * distance / (2 * 6371000.0);
        output->missile_altitude += earth_curvature * 0.01;
    }

    return ATOMIC_SERVICE_SUCCESS;
}